/*
	Peggy2Master code, used to convert serial data from a PC/Mac to
	TWI/I2C data for a Peggy 2.0 with the VideoPeggyTwi firmware based on
	serial interrupts. When not doing that, run internal graphics functions
	send them to the Peggy 2.0

	OJ - I want this to have one more trick. If there is nothing on the serial port,
	we run our own code here
	This probably means we should make the serial stuff into an interrupt and keep
	our own code on the normal stuff. NOTE: The Wire.h library already occupies the interrupts.
	We can still do a timing-based interrupt, but I'm vary of the overhead....

	Note: we can use GIMP with PGM the format to get ASCCI files with the values we need.
	This produces (I think) 8bit grayscale. Make perl script to convert it into 4-bit
	 If you can set "Maxval" to 15 for PGM in Gimp, it would help a lot!
	 /Users/olesk/Dropbox/Electronics/Arduino/Code/WallySentry/MegaMan/MegaMan.h
	 What this code does NOT:
	 	- deal with servos or sensors (this is Wally's job!)

	UPDATE: The hardware has changed to an Arduiono MEGA 2560

*/

#include <Wire.h>						// I2C and serial communication
#include <SPI.h>						// SPI communication
#include <SdFat.h>						//Add the SdFat Libraries
#include <SdFatUtil.h>
#include <SFEMP3Shield.h>				//and the MP3 Shield Library

#include "MegaMan.h"					// This program's h-file
#include "flash_pics.h"					// this is where I want to put all my reusable stuff
#include "p2anim.h"						// animations & effects -- DEBUG DEBUG - RAM hog?

SFEMP3Shield MP3player;					//create and name the library object

#define PEGGY_ADDRESS 34				// The Peggy 2.0's address on the I2C bus
#define TWI_FREQ 300000					// The I2C/TWO bus frequency, 300'000 bits/second

uint8_t header[6] = { 0xde,0xad,		// 0xdeadbeef is the initialization string of the Peggy 2.0. Once it gets this
					  0xbe,0xef,1,0};	// it will read data from I2C directly to the local framebuffer

uint8_t globalCounter;					// A global counter used to keep persisten values between function calls
boolean globalFlipFlop = 1;				// A global flag used to keep persisten values between function calls

// Timer interrupt variable definitions (defined static so they will retain value between function calls)
static const uint16_t tics_per_msec     = ((F_CPU / 1000) / 8);	// Part of our timer interpt (16000000 / 1000 / 8 = 2000
static volatile uint16_t time_sec;		// Notice volatile definition
static volatile uint16_t time_msec;
static volatile uint8_t heartBeatCounter;

// XBee communication variables
boolean abortFlag;				// Flag that if set interrupts any ongoing animations (set in timer interrupt when XBee message received)s
uint8_t sender = 9;				// The sender who's contacting us
uint8_t cmd = 9;				// The sender's requested command
char *cmdParam;					// The sender's command parameters


/* 	Timer interrupt
	Used primarily to check that the UART has been quiet for a while before whitching to our own
	graphics afterhaving done UART-SPI conversion
*/

ISR (TIMER5_COMPA_vect)	// We use TIMER5 on this Arduino MEGA 2560 to avoid collisions with built-in Arduino libraries using TIMER1-3
{						// We probably do not need msec, reduce frequency - we only need 1 second or 0.1 seconds
	time_msec++;
	if (time_msec >= 1000) {
		time_msec = 0;
		time_sec++;
		heartBeatCounter++;
		if(heartBeatCounter == 1)				// Triggering every second
			{
			//Serial1.print("{s1:c0:ALIVE}");		// Sending heartbeat every 10 seconds
			//Serial.print(time_sec);
			//Serial.println(" - Sending heartbeat on XBee");
			if (Serial1.available() && processXbeeMsg()) abortFlag = 1;
			heartBeatCounter = 0;
			}

	}  
}

void videoTransfer(uint8_t count)
{
	//uint8_t count = Serial.available();	// How many character on the UART input buffer...

	if (count > 0) 			// ...more than one, so we start reading the UART
		{					// dont allow send too many bytes at once, dont want to exceed the buffer
							// size of the Wire library
			if (count > 16) count = 16;

			Wire.beginTransmission(PEGGY_ADDRESS);
			while (count-- > 0 )
			{
				uint8_t c = Serial.read();
				Wire.write(c);
			}
			Wire.endTransmission();

	}
}

/*
	This function takes an image and copies to the frameBuffer.
	NOTE: The least significan bits and most significant bits are swaped on Peggy 2.0!
	This means that decimal 15 (b00001111) will light up the LEFT LED, and keep the right LED off
	When processing this we need to swap nibbles
*/
// VERY INEFFICIENT - LETS WRITE THE DATA DIRECTLY WITHOUT GOING VIA THE FRAMEBUFFER!
void imageFromFlash(char *picture)
{
	// Get image from flash
	uint16_t i = 0;
	for (i = 0; i < 325; i++) {
		frameBuffer[i] = pgm_read_byte_near(picture + i);
		}
		sendBuffer(&frameBuffer[0]);	// Start transmitting the frameBuffer to Peggy 2.0 over the I2C bus

// 	Serial.println(F("Writing to I2C...."));
// 	Wire.beginTransmission(PEGGY_ADDRESS);
// //	Get image from flash
// 	uint16_t i = 0;
// 	for (i = 0; i < 325; i++) {
// 		Wire.write(pgm_read_byte_near(picture + i));
// 	}
// 	Wire.endTransmission();
}



/* 	Takes a pixel pos (0..24, 0..24) and a gradient (0..15) and sends to Peggy 2.0
	Needs optimization - very inefficient
*/
void putPixel(uint8_t x, uint8_t y, uint8_t gradient)
{
    // We need to change from x pixels, to x_byte byte;
    //	Bytes: 	 	 0	  1	   2  ....
    //  Pixels:		0|1  2|3  4|5 ....

    int pos;
    if (x % 2 == 0) 													// We have an *even* x coordinate for the pixel - i.e. leftmost in the byte (MSB)
    {
     	pos = (x / 2) + (y * 13);										// Convert pixel pos to byte number (for the row)
     	setLeftN(frameBuffer[pos], gradient);							// Inserting nibble into leftmost part of byte (MACRO)


    } else					 											// We have an odd number of pixels
    {
		x--;															// We subtract one to get the right nibble address (beginning of the byte we need)
		pos = (x / 2) + (y * 13);										// Convert pixel pos to byte number (for the row)
		setRightN(frameBuffer[pos], gradient);							// Inserting nibble into rightmost part of byte (MACRO)
    }

	sendBuffer(frameBuffer);	// Start transmitting the frameBuffer to Peggy 2.0 over the I2C bus
}



/*	This function fills the frameBuffer with some pattern/graphic/text and finally calls
	the sendBuffer function to transmit the whole buffer to Peggy2
	Valid parameters:
		"testpattern"	- draw pattern of straight lines that fade in and out
		"testtext"		- writes "Hello World!" in scrolling text
		"picture"	- puts the picture that is currently in the flash buffer on screen
		"clrscr"		- turns all LEDs off
		"allon"			- turns all LEDs on
*/
void updateBuffer(char update[])
{

	uint8_t update_nr = 0;

	if (!strcmp(update, "testpattern")) update_nr = 1;
	if (!strcmp(update, "testtext")) update_nr = 2;
	if (!strcmp(update, "clrscr"))  update_nr = 3;
	if (!strcmp(update, "allon"))  update_nr = 4;

	switch(update_nr)
	{
		case 1:	{	// Draw testpattern ("checkerboard" 01010101... pattern that fades in and out)

			uint8_t xres = 13;
			uint8_t yres = 25;

			uint8_t *ptr = frameBuffer;	// ptr now points to the frameBuffer array

			uint8_t valFirst;
			uint8_t valSecond;
			valFirst = globalCounter;	// Need a global var to keep track of LED strengths between calls
			globalCounter = globalCounter + globalFlipFlop;
			if (globalCounter == 15) globalFlipFlop = -1;
			if (globalCounter == 0) globalFlipFlop = 1;
			valSecond = 0; 				// Every second row always off

			for (uint8_t y =0; y < yres; y++)
			{
				for (uint8_t  x = 0; x < xres; x++)
				{
					*ptr++ = valFirst | (valSecond << 4);
       				//frameBuffer[(y * xres) + x] = valFirst | (valSecond << 4);
    			}
			}
		}

			sendBuffer(&frameBuffer[0]);	// Start transmitting the frameBuffer to Peggy 2.0 over the I2C bus
			break;

		case 2:	// Write "Hello world!"
			break;

		case 3: // All LEDs off (clear screen)
			memset(&frameBuffer[0], 0, 13*25);
			sendBuffer(&frameBuffer[0]);	// Start transmitting the frameBuffer to Peggy 2.0 over the I2C bus
			break;

		case 4: // All LEDs on
			memset(&frameBuffer[0], 255, 13*25);
			sendBuffer(&frameBuffer[0]);	// Start transmitting the frameBuffer to Peggy 2.0 over the I2C bus
			break;

	}

}

/*	This function manipulates the frameBuffer with some effect and finally calls
	the sendBuffer function to transmit the whole buffer to Peggy2
	Valid parameters:
		"fall_down"		- the picture currently on the Peggy 2.0 "falls off" the screen
		"fade_out"		- the picture currently on the Peggy 2.0 fades to black
		"fade_in"		- the picture from flash is being faded in to the buffer from black
*/
void alterBuffer(char effect[], char *picture) {

	uint8_t effect_nr = 0;

	if (!strcmp(effect, "fall_down")) effect_nr = 1;
	if (!strcmp(effect, "fade_out"))  effect_nr = 2;
	if (!strcmp(effect, "fade_in"))  effect_nr = 3;

	switch(effect_nr) {
		case 1:	{	// Make current frameBuffer "fall out fo the screen" (scroll it down and add 0 to the top)
			for (uint8_t i = 0; i < 25; i++) {	// We need to do it 25 times to make sure we scroll out everything
				for (uint8_t j = 25; j > 0; j--) {
					//memcpy(&frameBuffer[0], &frameBuffer[13], 13*25);
					memcpy(&frameBuffer[13*(j)], &frameBuffer[13*(j-1)], 13);
				}
				// we now need to fill an extra empty line from the top
				memset(&frameBuffer[0], 0, 13);
				sendBuffer(&frameBuffer[0]);	// Start transmitting the frameBuffer to Peggy 2.0 over the I2C bus
				delay(100);	// Just to see what we're doing
				}
			}
			break;

		case 2: {		// Fade out to black
			uint8_t xres = 13;
			uint8_t yres = 25;
			uint8_t leftmost = 0;
			uint8_t rightmost = 0;
			for(uint8_t gradient = 0; gradient < 16; gradient++) {
				for (uint8_t y =0; y < yres; y++) {
					for (uint8_t  x = 0; x < xres; x++) {
						// Reduce value of each 4bit nibble by one
						leftmost = frameBuffer[(y*13)+x] >> 4 & 0x0F;
						rightmost = frameBuffer[(y*13)+x] & 0x0F;
						if (leftmost > 0) leftmost--;
						if (rightmost > 0) rightmost--;
						frameBuffer[(y*13)+x] = (leftmost << 4) | rightmost;
						}
					}
				sendBuffer(&frameBuffer[0]);	// Start transmitting the frameBuffer to Peggy 2.0 over the I2C bus
				delay(100);
				}
			}
			break;

		case 3: {		// Fade in from black
			uint8_t xres = 13;
			uint8_t yres = 25;
			uint8_t leftmost = 0;
			uint8_t rightmost = 0;

			uint8_t pixel;
			uint8_t current_level;

			for(current_level = 0; current_level < 16; current_level++) {
				for (uint8_t y = 0; y < yres; y++) {
					for (uint8_t  x = 0; x < xres; x++) {
						pixel = pgm_read_byte_near(picture + (y*13)+x);
						leftmost = pixel >> 4 & 0x0F;		// NOTE: Should be leftmost
						rightmost = pixel & 0x0F;			// NOTE: should be rightmost

						if (leftmost > current_level) leftmost = current_level;
						if (rightmost > current_level) rightmost = current_level;
						frameBuffer[(y*13)+x] = (leftmost << 4) | rightmost;
					}
				}

				sendBuffer(&frameBuffer[0]); 	// Start transmitting the frameBuffer to Peggy 2.0 over the I2C bus
				delay(100);
				}

			}
			break;
		}
}

/*
	This is the rather RAM wasteful approach to updating the Peggy 2.0 display.
	The function requires a pointer to a uint8_t/char buffer to transmit in one
	big gulp. It should ideally be used as little as possible - it would be much
	better to simply write to the Peggy 2.0 directly without using a full
	frameBuffer every time
*/
void sendBuffer(uint8_t *bufferptr)
{
	uint8_t *ptr = header;
	int count = sizeof(header);

	Wire.beginTransmission(PEGGY_ADDRESS);
	while (count--)
	{
		Wire.write(*ptr++);
	}
	Wire.endTransmission();

	count = 25;

	while (count-- )
	{
		Wire.beginTransmission(PEGGY_ADDRESS);
		Wire.write(bufferptr,13);
		bufferptr+=13;
		Wire.endTransmission();
	}
}


/*
	This function receives messages on this format: {s0c1:Alive.mp3}
	Format: s?			= numeric ID of sender (0 = Wally)
			c?:param	= command number with argument (max length = 64 chars)
							list of commands:
						0: event notification	: description of event
						1: play MP3 file		: full filename of MP3 file
						2: display text			: text
						3: play animation		: animation name
						4: not implemented		: not implemented
						9: heartbeat			: heartbeat

	Returns 0 if message is heartbeat, 1 otherwise - NOTE: MAKE THIS SMARTER!

	We ack the messages by retuning "ack:c:param" on the serial line, or if malformed
	message/illegal format: "ERR"

	Serial.read must be replaced with Serial1.read for live runs with Xbee
*/
uint8_t processXbeeMsg(void)
{
	uint8_t returnValue = 1;	// Set to zero if we're getting a hearbeat

	boolean started = false;	// Have we received an opening curly bracket?
	boolean ended = false;		// Have we received and closing curly bracket?
	boolean msgOK = true;		// Is the message understandable and syntactically correct?

	char inData[128]; 			// Max length of UART buffer is 128 byte
	byte index = 0;


	while(Serial1.available() > 0)				// We have data waiting for us in the UART buffer
	{
		char inChar = Serial1.read();
		if (!started && inChar !='{') continue;	// Until we have a start message we skip

		if(inChar == '{')						// The beginning of a message
		{
			started = true;
			index = 0;							// We've found start of message - resetting index
			inData[index] = '\0';				// We replace the curly bracket with a zero char
		}
		else if(inChar == '}')					// The end of a message
		{
			ended = true;
			inData[index] = '\0';				// We replace the curly bracket with a zero char
			break;
		}
		else									//
		{
			if(index < 128-1)					// Array size
			{
				inData[index++] = inChar;		// Current character added to array
				inData[index] = '\0';			// We add a zero after current char (string termination)
			}
		}

	}
	// We now have gotten the full message
	if (started && (inData[0] != 's' || inData[3] != 'c' || inData[5] != ':'))	// Malformed message
		{
		Serial.println("Message malformed. Ignoring...");
		Serial1.print("ERR");
		returnValue = 0;	// We do not interrupt what we're doing if we get malformed messages, just alert the sender so he can try again
	} else		// Message ok
	{
		Serial1.print("ACK");
		sender = inData[1] - (65-17);	//Converting form ASCII
		cmd = inData[4] - (65-17);
		cmdParam = &inData[6];			// Points to beginning of argument string

		/*Serial.print("Sender: ");
		Serial.print(sender);
		Serial.print(", Command: ");
		Serial.print(cmd);
		Serial.print(", Argument: ");
		while(*cmdParam != '\0')
		{
			Serial.print(*cmdParam++);
		}
		Serial.println(".");
		*/
	}

	// Prepare return value depending on message being a heartbeat or not
	if(cmd == 9) returnValue = 0;
	return returnValue;
}


void setup()
{
	delay(1000);			// Giving the Peggy 2.0 time to get ready
	Serial.begin(115200);	// Setting 115200 baud rate for UART for debug messages
	Serial1.begin(9600);	// Setting 115200 baud rate for UART1 for XBee communication
	Serial.println();
	Serial.println(F(" *** I LIVE AGAIN ***"));	// Note! The F() reads from flash and saves RAM


	Serial.print(F("Initializing I2C communication..."));
	Wire.begin();			// Initializing I2C
	pinMode(A4, INPUT);		// set pin A4 to input (for I2C)
	digitalWrite(A4, HIGH);	// turn on pullup resistors on A4 (for I2C)
	pinMode(A5, INPUT);		// set pin A5 to input (for I2C)
	digitalWrite(A5, HIGH);	// turn on pullup resistors on A5 (for I2C)
	// jack up the frequency for I2C (TWI), we need a pretty high rate  from the TWI engine for this to handle 115k input
	// without getting buffer overruns
	TWSR &= ~(1<<TWPS0);
	TWSR &= ~(1<<TWPS1);
	TWBR = ((F_CPU / TWI_FREQ) - 16) / 2;
	Serial.println(F("DONE"));

	delay(100);
	Serial.print(F("Initializing MP3 Player Shield..."));
	uint8_t result = MP3player.begin();
	if(result != 0)	// Error...
	{
		// 0 OK
		// 1 SD Card Init Failure
		// 2 SD Card File System (FAT) init failure
		// 3 SD Card Root Directory init failure
		// 4 MP3 Decoder mode failure
		// 5 MP3 Decoder speed failure
    	Serial.print(F("Error code: "));
    	Serial.print(result);
    	Serial.println(F(" when trying to start MP3 player"));
    	exit(0);
    }
   	MP3player.SetVolume(0,0);
   	MP3player.playMP3("Hi.mp3");	// Saying hello
    Serial.println(F("DONE"));

    Serial.println(F("Initializing timer..."));

	cli();	// Disable global interrupts
	// The double buffered Output Compare Register (OCR5A and OCR5B) are compared with the
	// Timer/Counter value at all times. The compare match event will also set the
	// Compare Flag (OCF5A or OCF5B) which can be used to generate an Output Compare interrupt request

	TCCR5A = 0x00; // No output - CTC mode (Clear Timer on Compare match)
	TCCR5B = 0x0a; // 0000 1010 - CTC (OCR1A), f/8 prescaler (enabling WGM52 and CS51 bits in the TCCR5B registry)

	// Set OCR5A register to 46875 (0b10110111 00011011)- once every second with 16MHz and clk/8 prescaler
	//                                 ^^OCR5AH  ^^OCR5AL
	OCR5AH = (tics_per_msec >> 8) & 0xff;	// Output Compare Register A, High byte for TIMER5 (0xff = 0b11111111)
	OCR5AL = (tics_per_msec >> 0) & 0xff;	// Output Compare Register A, Low byte for TIMER5
			// NOTE: Do we really need to AND these values???
			// NOTE: Verify that this is actually correct for 16MHz (46875)

	// Interrupt mask register - interrupt on OCR1 match
	TIMSK5 = 0x02;	// Timer/Counter5 Interrupt Mask Register set to clk/8 prescaler (timer runs on 16Mhz/8=2MHz)

	sei();	// Enable global interrupts

	Serial.println(F("DONE"));
	updateBuffer("clrscr");

}


/* Main loop
	Please note: the serial to I2C conversion does NOT touch the frameBuffer. This means that we cannot
	play with whatever is currently being displayed at the Peggy 2.0 as this is unknown to us
	This could be circumvented by copying the serial input to the Peggy 2.0 via the frameBuffer (slow!)
	or we could query the Peggy 2.0 for the current contents of the "screen" (unimplemented on both sides)
*/
void loop()
{
	//if (Serial1.available()) processXbeeMsg();	// We got messages waiting  NOW DONE IN TIMER INTERRUPT

	//Serial.println("Displaying testpattern...");
	//imageFromFlash(testpattern);
	//p2animScrollText("HELLO");
	//Serial1.print("{s1:c0:ALIVE}");
	//Serial1.print("XXXXXXXXXXXXX");
	if(abortFlag)
	{
		abortFlag = 0;
		Serial.println("ALARM!");
		p2animScrollText(cmdParam);
	}

}


